#include "MagneticSensorI2C.h"

/** Typical configuration for the 12bit AMS AS5600 magnetic sensor over I2C interface */
MagneticSensorI2CConfig_s AS5600_I2C = {
    .chip_address = 0x36,
    .bit_resolution = 12,
    .angle_register = 0x0C,
    .data_start_bit = 11
};

/** Typical configuration for the 12bit AMS AS5048 magnetic sensor over I2C interface */
MagneticSensorI2CConfig_s AS5048_I2C = {
    .chip_address = 0x40,  // highly configurable.  if A1 and A2 are held low, this is probable value
    .bit_resolution = 14,
    .angle_register = 0xFE,
    .data_start_bit = 15
};


// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
//  @param _chip_address  I2C chip address
//  @param _bit_resolution  bit resolution of the sensor  
//  @param _angle_register_msb  angle read register
//  @param _bits_used_msb number of used bits in msb
MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb,
                                     int _bits_used_msb)
{
    // chip I2C address
    chip_address = _chip_address;
    // angle read register of the magnetic sensor
    angle_register_msb = _angle_register_msb;
    // register maximum value (counts per revolution)
    cpr = pow(2, _bit_resolution);

    // depending on the sensor architecture there are different combinations of
    // LSB and MSB register used bits
    // AS5600 uses 0..7 LSB and 8..11 MSB
    // AS5048 uses 0..5 LSB and 6..13 MSB
    // used bits in LSB
    lsb_used = _bit_resolution - _bits_used_msb;
    // extraction masks
    lsb_mask = (uint8_t) ((2 << lsb_used) - 1);
    msb_mask = (uint8_t) ((2 << _bits_used_msb) - 1);
    wire = &Wire;
}

MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config)
{
    chip_address = config.chip_address;

    // angle read register of the magnetic sensor
    angle_register_msb = config.angle_register;
    // register maximum value (counts per revolution)
    cpr = pow(2, config.bit_resolution);

    int bits_used_msb = config.data_start_bit - 7;
    lsb_used = config.bit_resolution - bits_used_msb;
    // extraction masks
    lsb_mask = (uint8_t) ((2 << lsb_used) - 1);
    msb_mask = (uint8_t) ((2 << bits_used_msb) - 1);
    wire = &Wire;
}

void MagneticSensorI2C::init(TwoWire *_wire)
{

    wire = _wire;

    // I2C communication begin
    wire->begin();

    // velocity calculation init
    angle_prev = 0;
    velocity_calc_timestamp = _micros();

    // full rotations tracking number
    full_rotation_offset = 0;
    angle_data_prev = getRawCount();
}

//  Shaft angle calculation
//  angle is in radians [rad]
float MagneticSensorI2C::getAngle()
{
    // raw data from the sensor
    float angle_data = getRawCount();

    // tracking the number of rotations
    // in order to expand angle range form [0,2PI]
    // to basically infinity
    float d_angle = angle_data - angle_data_prev;
    // if overflow happened track it as full rotation
    if (abs(d_angle) > (0.8 * cpr)) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI;
    // save the current angle value for the next steps
    // in order to know if overflow happened
    angle_data_prev = angle_data;
    // return the full angle
    // (number of full rotations)*2PI + current sensor angle
    return (full_rotation_offset + (angle_data / (float) cpr) * _2PI);
}

// Shaft velocity calculation
float MagneticSensorI2C::getVelocity()
{
    // calculate sample time
    unsigned long now_us = _micros();
    float Ts = (now_us - velocity_calc_timestamp) * 1e-6;
    // quick fix for strange cases (micros overflow)
    if (Ts <= 0 || Ts > 0.5) Ts = 1e-3;

    // current angle
    float angle_c = getAngle();
    // velocity calculation
    float vel = (angle_c - angle_prev) / Ts;

    // save variables for future pass
    angle_prev = angle_c;
    velocity_calc_timestamp = now_us;
    return vel;
}


// function reading the raw counter of the magnetic sensor
int MagneticSensorI2C::getRawCount()
{
    return (int) MagneticSensorI2C::read(angle_register_msb);
}

// I2C functions 
/*
* Read a register from the sensor
* Takes the address of the register as a uint8_t
* Returns the value of the register
*/
int MagneticSensorI2C::read(uint8_t angle_reg_msb)
{
    // read the angle register first MSB then LSB
    byte readArray[2];
    uint16_t readValue = 0;
    // notify the device that is aboout to be read
    wire->beginTransmission(chip_address);
    wire->write(angle_reg_msb);
    wire->endTransmission(false);

    // read the data msb and lsb
    wire->requestFrom(chip_address, (uint8_t) 2);
    for (byte i = 0; i < 2; i++)
    {
        readArray[i] = wire->read();
    }

    // depending on the sensor architecture there are different combinations of
    // LSB and MSB register used bits
    // AS5600 uses 0..7 LSB and 8..11 MSB
    // AS5048 uses 0..5 LSB and 6..13 MSB
    readValue = (readArray[1] & lsb_mask);
    readValue += ((readArray[0] & msb_mask) << lsb_used);
    return readValue;
}

/*
* Checks whether other devices have locked the bus. Can clear SDA locks.
* This should be called before sensor.init() on devices that suffer i2c slaves locking sda 
* e.g some stm32 boards with AS5600 chips
* Takes the sda_pin and scl_pin
* Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW
*/
int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin)
{

    pinMode(scl_pin, INPUT_PULLUP);
    pinMode(sda_pin, INPUT_PULLUP);
    delay(250);

    if (digitalRead(scl_pin) == LOW)
    {
        // Someone else has claimed master!");
        return 1;
    }

    if (digitalRead(sda_pin) == LOW)
    {
        // slave is communicating and awaiting clocks, we are blocked
        pinMode(scl_pin, OUTPUT);
        for (byte i = 0; i < 16; i++)
        {
            // toggle clock for 2 bytes of data
            digitalWrite(scl_pin, LOW);
            delayMicroseconds(20);
            digitalWrite(scl_pin, HIGH);
            delayMicroseconds(20);
        }
        pinMode(sda_pin, INPUT);
        delayMicroseconds(20);
        if (digitalRead(sda_pin) == LOW)
        {
            // SDA still blocked
            return 2;
        }
        _delay(1000);
    }
    // SDA is clear (HIGH)
    pinMode(sda_pin, INPUT);
    pinMode(scl_pin, INPUT);

    return 0;
}
